#include "Triangulator.h"
#include "Util.h"

#include "..\MathAux\imagelib\triangulate.h"

using namespace cv;

struct  STriangulateResidualData
{
	CCamera* CamA;
	CCamera* CamB;
	Point2d p;
	Point2d q;

	CCamera** ppCams;
	double* points;
	int NumPoints;
};

STriangulateResidualData g_TriangulateResidualData;

void triangulation_residual(const int *m, const int *n, double *x, 
	double *fvec, double *iflag) 
{
	/* Project the point into the two views */
	Point2d p = g_TriangulateResidualData.CamA->Project(x, false, false);
	Point2d q = g_TriangulateResidualData.CamB->Project(x, false, false);

	fvec[0] = g_TriangulateResidualData.p.x - p.x;
	fvec[1] = g_TriangulateResidualData.p.y - p.y;
	fvec[2] = g_TriangulateResidualData.q.x - q.x;
	fvec[3] = g_TriangulateResidualData.q.y - q.y;
}

void triangulation_n_residual(const int *m, const int *n, double *x, 
	double *fvec, double *iflag) 
{
	/* Project the point into the two views */
	for (int i = 0; i < g_TriangulateResidualData.NumPoints; i++)
	{
		CCamera* pCam = g_TriangulateResidualData.ppCams[i];
		Point2d p = pCam->Project(x, false, false);
		fvec[2*i] = g_TriangulateResidualData.points[2*i] - p.x;
		fvec[2*i + 1] = g_TriangulateResidualData.points[2*i + 1] - p.y;
	}
}

Point3d CTriangulator::TriangulateBundler(double* p, double* q, Mat R1, Mat t1, Mat R2, Mat t2, double* error)
{
	v2_t pb = *CastToBundlerVec<v2_t>(p);
	v2_t qb = *CastToBundlerVec<v2_t>(q);

	v3_t point = triangulate(pb, qb, GetMatrixDataAsDouble(R1), GetMatrixDataAsDouble(t1), GetMatrixDataAsDouble(R2), GetMatrixDataAsDouble(t2),
		error);

	return Point3d(point.p[0], point.p[1], point.p[2]);
}

Point3d CTriangulator::RefineTriangulateNonLinear(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB, Point3d _x)
{
	g_TriangulateResidualData.CamA = &CamA; 
	g_TriangulateResidualData.CamB = &CamB;
	g_TriangulateResidualData.p = p;
	g_TriangulateResidualData.q = q;

	double x[] = {_x.x , _x.y, _x.z};

	lmdif_driver(triangulation_residual, 4, 3, x, 1.0e-10);
	
	return Point3d(x[0], x[1], x[2]);
}

Point3d CTriangulator::TriangulateInternal(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB)
{
	Point3d TrinagulatedPoint = TriangulateLinear(p , q, CamA, CamB);
	Point3d TriangulatedRefined = RefineTriangulateNonLinear(p , q, CamA, CamB, TrinagulatedPoint);
	return TriangulatedRefined;
}

Point3d CTriangulator::TriangulateLinear(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB)
{
	double A[12];
	double b[4];
	double x[3];


	SCameraParams CamAParams = CamA.GetParameters();
	SCameraParams CamBParams = CamB.GetParameters();

	double* R0 = GetMatrixDataAsDouble(CamAParams.R);
	double* R1 = GetMatrixDataAsDouble(CamBParams.R);
	double* t0 = GetMatrixDataAsDouble(CamAParams.t);
	double* t1 = GetMatrixDataAsDouble(CamBParams.t);

	A[0] = R0[0] - p.x * R0[6];  
	A[1] = R0[1] - p.x * R0[7];  
	A[2] = R0[2] - p.x * R0[8];

	A[3] = R0[3] - p.y * R0[6];  
	A[4] = R0[4] - p.y * R0[7];  
	A[5] = R0[5] - p.y * R0[8];

	A[6] = R1[0] - q.x * R1[6];  
	A[7] = R1[1] - q.x * R1[7];  
	A[8] = R1[2] - q.x * R1[8];

	A[9] = R1[3] - q.y * R1[6];  
	A[10] = R1[4] - q.y * R1[7];  
	A[11] = R1[5] - q.y * R1[8];

	b[0] = t0[2] * p.x - t0[0];
	b[1] = t0[2] * p.y - t0[1];
	b[2] = t1[2] * q.x - t1[0];
	b[3] = t1[2] * q.y - t1[1];

	dgelsy_driver(A, b, x, 4, 3, 1);

	return Point3d(x[0], x[1], x[2]);
}

Point3d CTriangulator::Triangulate(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB)
{
	SCameraParams CamAParams = CamA.GetParameters();
	SCameraParams CamBParams = CamB.GetParameters();

	Mat p3D = INIT_MAT(3, 1, p.x , p.y, -1);
	Mat q3D = INIT_MAT(3, 1, q.x , q.y, -1);
	
	Mat p3DNormalized = CamAParams.K.inv() * p3D;
	Mat q3DNormalized = CamBParams.K.inv() * q3D;

	Mat p2DNormalized =  Utils::Project3D(p3DNormalized);
	Mat q2DNormalized =  Utils::Project3D(q3DNormalized);

	p2DNormalized = CamA.UndistortNormalizedPoint(p2DNormalized);
	q2DNormalized = CamB.UndistortNormalizedPoint(q2DNormalized);

	Point3d newPoint = TriangulateInternal(Utils::MatToVec<2>(p2DNormalized), Utils::MatToVec<2>(q2DNormalized), CamA	, CamB);

	return newPoint;
}

cv::Point3d CTriangulator::TrinagulateN( Point2d* pP, CCamera** ppCams, int NumPoints, double& error )
{
	double* pP2DNormalized = new double[NumPoints * 2];
	for (int i = 0; i < NumPoints; i++)
	{
		SCameraParams CamParams = ppCams[i]->GetParameters();
		Point2d p = pP[i];
		Mat p3D = INIT_MAT(3, 1, p.x , p.y, -1.0);

		Mat p3DNormalized = CamParams.K.inv() * p3D;

		Mat p2DNormalized =  Utils::Project3D(p3DNormalized);
		
		p2DNormalized = ppCams[i]->UndistortNormalizedPoint(p2DNormalized);

		memcpy(pP2DNormalized + 2*i , GetMatrixDataAsDouble(p2DNormalized) , sizeof(double) * 2);
	}

	Point3d p3D = TriangulateNInternal(pP2DNormalized, ppCams, NumPoints);

	//compute error of reprojection
	error = 0.0;

	for (int i = 0; i < NumPoints; i++)
	{
		double p3[] = {p3D.x, p3D.y, p3D.z};
		
		Point2d q = ppCams[i]->Project(p3, true, true) * -1.0;
		Point2d p = pP[i];
		Point2d p_q = p-q;
		error += p_q.x*p_q.x + p_q.y*p_q.y;
	}

	error = sqrt(error / NumPoints);

	delete[] pP2DNormalized;
	return p3D;
}

Point3d CTriangulator::TriangulateNInternal(double* p, CCamera** ppCams, int NumPoints)
{
	Point3d TrinagulatedPoint = TriangulateNLinear(p , ppCams, NumPoints);
	Point3d TriangulatedRefined = RefineTriangulateNNonLinear(p , ppCams, NumPoints, TrinagulatedPoint);
	return TriangulatedRefined;
};

Point3d CTriangulator::TriangulateNLinear(double* p, CCamera** ppCams, int NumPoints)
{
	//todo: find a way to do the triangulate2 with triangulateN
	int NumEqs = 2 * NumPoints;
	int NumVars = 3; 

	double *A = (double *) malloc(sizeof(double) * NumEqs * NumVars);
	double *b = (double *) malloc(sizeof(double) * NumEqs);
	double *x = (double *) malloc(sizeof(double) * NumVars);

	for (int i = 0; i < NumPoints; i++) 
	{
		CCamera* pCam = ppCams[i];
		SCameraParams CamParams = pCam->GetParameters();
		double* R = GetMatrixDataAsDouble(CamParams.R);
		double* t = GetMatrixDataAsDouble(CamParams.t);

		
		int row = 6 * i;
		int brow = 2 * i;

		A[row + 0] = R[0] - p[2*i] * R[6];  
		A[row + 1] = R[1] - p[2*i] * R[7];  
		A[row + 2] = R[ 2] - p[2*i] * R[8];

		A[row + 3] = R[3] - p[2*i + 1] * R[6];  
		A[row + 4] = R[4] - p[2*i + 1] * R[7];  
		A[row + 5] = R[5] - p[2*i + 1] * R[8];

		b[brow + 0] = t[2] * p[2*i] - t[0];
		b[brow + 1] = t[2] * p[2*i + 1] - t[1];
	}

	 dgelsy_driver(A, b, x, NumEqs, NumVars, 1);

	 return Point3d(x[0], x[1], x[2]);

	 delete[] A;
	 delete[] b;
	 delete[] x;
}

Point3d CTriangulator::RefineTriangulateNNonLinear(double* p, CCamera** ppCams, int NumPoints, Point3d _x)
{
	g_TriangulateResidualData.ppCams = ppCams; 
	g_TriangulateResidualData.points = p;
	g_TriangulateResidualData.NumPoints = NumPoints;

	int NumEqs = 2 * NumPoints;
	int NumVars = 3; 

	double x[] = {_x.x , _x.y, _x.z};

	lmdif_driver(triangulation_n_residual, NumEqs, NumVars, x, 1.0e-10);

	return Point3d(x[0], x[1], x[2]);
}